﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace MC
{
    public class RobotBase
    {
        MCLeisai mc;
        private TargetPos orig = new TargetPos();
        public TargetPos Orig { get => orig; set => orig = value; }
        public TargetPos Pixel2mm = new TargetPos(148.46, 148.46, 0);
        TargetPos Pulse2mm = new TargetPos(100, 100, 100);
        ushort[] Axis = new ushort[] { 0, 1 };
        double g_dAccel = 0.25; //ms
        public void Bind(MCLeisai mc)
        {
            this.mc = mc;
        }

        public TargetPos P2TPByOrig(Point p)
        {
            var pos = new TargetPos(orig.x - p.X / Pixel2mm.x, orig.y - p.Y / Pixel2mm.y);
            return pos;
        }
        public TargetPos P2TPByOrig(PointF p)
        {
            var pos = new TargetPos(orig.x - p.X / Pixel2mm.x, orig.y - p.Y / Pixel2mm.y);
            return pos;
        }
        public Point TP2PByOrig(TargetPos pos)
        {
            var p = new Point((int)((orig.x - pos.x) * Pixel2mm.x), (int)((orig.y - pos.y) * Pixel2mm.y));
            return p;
        }

        public Point TP2Point(TargetPos pos)
        {
            var p = new Point((int)(pos.x * Pixel2mm.x), (int)(pos.y * Pixel2mm.y));
            return p;
        }
        public TargetPos Point2TP(PointF pos)
        {
            var p = new TargetPos((pos.X / Pixel2mm.x), (pos.Y / Pixel2mm.y));
            return p;
        }

        public TargetPos GetCurPos()
        {
            var pos = new TargetPos();
            for (int i = 0; i < Axis.Length; i++)
            {
                pos[i] = mc.GetCurPos(Axis[i]) / Pulse2mm[i];
            }
            pos[2] = mc.GetCurPos(2) / Pulse2mm[2];
            return pos;
        }

        public void SetPosition(TargetPos pos)
        {
            for (int i = 0; i < Axis.Length; i++)
            {
                mc.SetPosition(Axis[i], (int)(pos[i] * Pulse2mm[i]));
            }
        }

        public void WaitMoveDone()
        {
            mc.WaitMoveDone(Axis);
        }

        public void Home(double v, bool bWait)
        {
            for (int i = 0; i < Axis.Length; i++)
            {
                mc.Home(Axis[i], 10, (int)(v * Pulse2mm[i]), g_dAccel);
            }
            mc.Home(2, 10, (int)(v * Pulse2mm[2]), g_dAccel);
            if (bWait)
                mc.WaitMoveDone(Axis);
            mc.WaitMoveDone(2);
            SetPosition(new TargetPos());
            mc.SetPosition(2, 0);
        }

        public void MoveAbs(TargetPos pos, double v, bool bWait = false)
        {
            for (int i = 0; i < Axis.Length; i++)
            {
                mc.MoveAbs(Axis[i], (int)(pos[i] * Pulse2mm[i]), 10, v * Pulse2mm[i], g_dAccel);
            }
            if (bWait)
                mc.WaitMoveDone(Axis);
        }

        public void MoveAbsAxis(int axis, double pos, double v, bool bWait = false)
        {
            if (axis < Axis.Length)
            {
                mc.MoveAbs(Axis[axis], (int)(pos * Pulse2mm[axis]), 10, v * Pulse2mm[axis], g_dAccel);
                if (bWait)
                    mc.WaitMoveDone(Axis[axis]);
            }

            else
            {
                mc.MoveAbs((ushort)axis, (int)(pos * Pulse2mm[axis]), 10, v * Pulse2mm[axis], g_dAccel);
                if (bWait)
                    mc.WaitMoveDone((ushort)axis);
            }
               
            
        }

        public void Stop(int axis)
        {
            mc.Stop(Axis[axis]);
        }

        public void StopALL(bool bWait = false)
        {
            for (int i = 0; i < Axis.Length; i++)
                mc.Stop(Axis[i]);
            mc.Stop(2);
            if (bWait)
                mc.WaitMoveDone(Axis);
        }
        public void SetDO(ushort bit, ushort on_off)
        {
            mc.SetDO(bit, on_off);
        }
    }

    public struct TargetPos
    {
        public TargetPos(double x = 0, double y = 0, double z = 0)
        {
            this.x = x; this.y = y; this.z = z;
        }
        public double x;
        public double y;
        public double z;

        public static TargetPos Parse(string text)
        {
            var pos = new TargetPos();
            try
            {
                string[] str = text.Split(',');
                if (str.Length == 2)
                {
                    int dx = int.Parse(str[0]);
                    int dy = int.Parse(str[1]);
                    pos.x = dx;
                    pos.y = dy;
                }
            }
            catch (Exception ex)
            {
            }
            return pos;
        }
        public override string ToString()
        {
            return $"{x},{y}";
        }
        public PointF ToPointF()
        {
            return new PointF((float)x, (float)y);
        }
        public static TargetPos operator -(TargetPos pos1, TargetPos pos2)
        {
            var target = new TargetPos(pos1.x - pos2.x, pos1.y - pos2.y, pos1.z - pos2.z);
            return target;
        }
        public double this[int i]
        {
            get
            {
                switch (i)
                {
                    case 0: return x;
                    case 1: return y;
                    case 2: return z;
                    default: return 0;
                }
            }
            set
            {
                switch (i)
                {
                    case 0: x = value; break;
                    case 1: y = value; break;
                    case 2: z = value; break;
                    default: break;
                }
            }
        }




    }
}
